#include "chassis_ctrl.h"

ChassisCtrl::ChassisCtrl()
{
    node = rclcpp::Node::make_shared("chassis_ctrl");
    rclcpp::QoS qos(rclcpp::KeepLast{7});
    // imu_sub_ = node->create_subscription<sensor_msgs::msg::Imu>("imu_raw",10,std::bind(&ChassisCtrl::ImuCallback, this, std::placeholders::_1));
    viyaw_sub_ = node->create_subscription<robot_msgs::msg::ViYaw>("ViYaw", 1000, std::bind(&ChassisCtrl::ViyawCallback, this,std::placeholders::_1));
}

ChassisCtrl::~ChassisCtrl()
{

}

void ChassisCtrl::work_loop()
{
    rclcpp::WallRate loop(100);
    while (rclcpp::ok()) {
        rclcpp::spin_some(node);
        loop.sleep();
  }
}

void ChassisCtrl::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu) {
    float ax,ay,az,x_radians,y_radians;
    ax = imu->linear_acceleration.x;
    ay = imu->linear_acceleration.y;
    az = imu->linear_acceleration.z;
    x_radians = atan2(ax, sqrt(ay*ay + az*az)); 
    double x_slope = x_radians * 180.0 / M_PI;
    y_radians = atan2(ay, sqrt(ax*ax + az*az)); 
    double y_slope = y_radians * 180.0 / M_PI;
    // RCLCPP_INFO(node->get_logger(),"x_radians = %f, y_radians = %f",x_radians,y_radians);
    x_data.push_back(x_slope);
    y_data.push_back(y_slope);
    if(x_data.size() > 50)
    {   
        double xslope = CalcValue(x_data);
        double yslope = CalcValue(y_data);
        RCLCPP_INFO(node->get_logger(),"xslope = %f, yslope = %f",xslope,yslope);
        x_data.clear();
        y_data.clear();
    }
}

double ChassisCtrl::CalcValue(std::vector<double> data)
{
    double min_value = *std::min_element(data.begin(), data.end());
    double max_value = *std::max_element(data.begin(), data.end());
    // 剔除过大或过小的值  
    std::vector<double> filtered_data;  
    for (double num : data) {  
        if (num >= min_value && num <= max_value) {  
            filtered_data.push_back(num);  
        }  
    }
    // 计算其余数据的平均值  
    double sum = std::accumulate(filtered_data.begin(), filtered_data.end(), 0.0);  
    return sum / filtered_data.size();
}

void ChassisCtrl::ViyawCallback(const robot_msgs::msg::ViYaw::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"yaw_value = %f",angles::to_degrees(msg->yaw_value));
}